template<class T>
DenseMatrix<T>::DenseMatrix()
{
    _rows = 0;
    _cols = 0;
    _data = NULL;
}

template<class T>
DenseMatrix<T>::DenseMatrix(UINT Size)
{
    _rows = 0;
    _cols = 0;
    _data = NULL;
    Allocate(Size, Size);
}

template<class T>
DenseMatrix<T>::DenseMatrix(UINT RowCount, UINT ColCount)
{
    _rows = 0;
    _cols = 0;
    _data = NULL;
    Allocate(RowCount, ColCount);
}

template<class T>
DenseMatrix<T>::DenseMatrix(const DenseMatrix<T>&M)
{
    _rows = 0;
    _cols = 0;
    _data = NULL;
    Allocate(M.RowCount(), M.ColCount());
    memcpy(_data, M._data, sizeof(T) * M.RowCount() * M.ColCount());
}

template<class T>
DenseMatrix<T>::~DenseMatrix()
{
    FreeMemory();
}

template<class T>
void DenseMatrix<T>::FreeMemory()
{
    //_data.FreeMemory();
    if(_data != NULL)
    {
        delete[] _data;
        _data = NULL;
    }
    _rows = 0;
    _cols = 0;
}

template<class T>
void DenseMatrix<T>::Allocate(UINT RowCount, UINT ColCount)
{
    if(_rows != RowCount || _cols != ColCount)
    {
        _rows = RowCount;
        _cols = ColCount;
        _data = new T[_rows * _cols];
        //_data.resize(_rows * _cols);
    }
}

template<class T>
void DenseMatrix<T>::WriteToStream(ostream &os, char delimiter) const
{
    for(UINT Row = 0; Row < _rows; Row++)
    {
        for(UINT Col = 0; Col < _cols; Col++)
        {
            os << Cell(Row, Col);
            if(Col != _cols - 1)
            {
                os << delimiter;
            }
        }
        os << endl;
    }
}

template<class T>
void DenseMatrix<T>::ExtractRow(UINT Row, vector<T> &result)
{
    result.resize(_cols);
    for(UINT Col = 0; Col < _cols; Col++)
    {
        result[Col] = _data[Row * _cols + Col];
    }
}

template<class T>
void DenseMatrix<T>::ExtractCol(UINT Col, vector<T> &result)
{
    result.resize(_rows);
    for(UINT Row = 0; Row < _rows; Row++)
    {
        result[Row] = _data[Row * _cols + Col];
    }
}

template<class T>
vector<T> DenseMatrix<T>::ExtractRow(UINT Row)
{
    vector<T> result(_cols);
    for(UINT Col = 0; Col < _cols; Col++)
    {
        result[Col] = _data[Row * _cols + Col];
    }
    return result;
}

template<class T>
vector<T> DenseMatrix<T>::ExtractCol(UINT Col)
{
    vector<T> result(_rows);
    for(UINT Row = 0; Row < _rows; Row++)
    {
        result[Row] = _data[Row * _cols + Col];
    }
    return result;
}

template<class T>
void DenseMatrix<T>::Clear(T Value)
{
    for(UINT Index = 0; Index < _rows * _cols; Index++)
    {
        _data[Index] = Value;
    }
}

template<class T>
DenseMatrix<T>& DenseMatrix<T>::operator = (const DenseMatrix<T>&M)
{
    Allocate(M.RowCount(), M.ColCount());
    memcpy(_data, M._data, sizeof(T) * M.RowCount() * M.ColCount());
    return (*this);
}

template<class T>
DenseMatrix<T> DenseMatrix<T>::OuterProduct(const vector<T> &A, const vector<T> &B)
{
    Assert(A.size() == B.size() && A.size() != 0, "Invalid vector dimensions in DenseMatrix<T>::OuterProduct");
    const UINT Size = A.size();

    DenseMatrix result, AMat(Size, 1), BMat(1, Size);
    for(UINT Index = 0; Index < Size; Index++)
    {
        AMat[Index][0] = A[Index];
        BMat[0][Index] = B[Index];
    }
    DenseMatrix::Multiply(result, AMat, BMat);
    return result;
}

template<class T>
T DenseMatrix<T>::CompareMatrices(const DenseMatrix<T> &left, const DenseMatrix<T> &right)
{
    T result = T(0.0);
    UINT RowCount = left.RowCount();
    UINT ColCount = left.ColCount();
    for(UINT Row = 0; Row < RowCount; Row++)
    {
        for(UINT Col = 0; Col < ColCount; Col++)
        {
            result += Math::Abs(left[Row][Col] - right[Row][Col]);
        }
    }
    return result;
}

template<class T>
void DenseMatrix<T>::LUSolve(vector<T> &x, const vector<T> &b)
{
    Assert(Square() && b.size() == _rows, "Invalid paramater DenseMatrix<T>::LUSolve");

    vector<T> y;
    x.resize(b.size());
    y.resize(b.size());

    DenseMatrix<T> L, U, Copy;
    Copy = *this;
    Copy.LUDecomposition(L, U);

    for(UINT i = 0; i < _rows; i++)
    {
        T Sum = 0.0f;
        for(UINT j = 0; j < i; j++)
        {
            Sum += L[i][j] * y[j];
        }
        y[i] = b[i] - Sum;
    }

    for(int i = _rows - 1; i >= 0; i--)
    {
        T Sum = 0.0;
        for(int j = i + 1; j < int(_rows); j++)
        {
            Sum += U[i][j] * x[j];
        }
        x[i] = (y[i] - Sum) / U[i][i];
    }
}

template<class T>
void DenseMatrix<T>::operator += (const DenseMatrix &M)
{
    Assert(M._rows == _rows && M._cols == _cols, "Invalid matrix dimensions");
    for(UINT Row = 0; Row < _rows; Row++)
    {
        for(UINT Col = 0; Col < _cols; Col++)
        {
            (*this)[Row][Col] += M[Row][Col];
        }
    }
}

template<class T>
void DenseMatrix<T>::LUDecomposition(DenseMatrix<T> &L, DenseMatrix<T> &U)
{
    Assert(Square(), "DenseMatrix<T>::LUDecomposition called on non-square matrix");
    L.Identity(_rows);
    U.Identity(_rows);
    for(UINT k = 0; k < _rows; k++)
    {
        U[k][k] = Cell(k, k);
        for(UINT i = k; i < _rows; i++)
        {
            L[i][k] = Cell(i, k) / U[k][k];
            U[k][i] = Cell(k, i);
        }
        for(UINT i = k; i < _rows; i++)
        {
            for(UINT j = k; j < _rows; j++)
            {
                Cell(i, j) -= L[i][k] * U[k][j];
            }
        }
    }
}

template<class T>
bool DenseMatrix<T>::EigenSystemVictor(vector<T> &Eigenvalues, DenseMatrix<T> &Eigenvectors) const
{
    PersistentAssert(Square(), "Eigensystem requires a square matrix");
    const UINT N = _rows;
    std::complex<double> *A = new std::complex<double>[N * N];
	
    for(UINT Col = 0; Col < N; Col++)
	{
        for(UINT Row = 0; Row < N; Row++)
        {
			A[Col * N + Row] = std::complex<double>(Cell(Row, Col), 0.0);
		}
	}

    std::complex<double> *evals = new std::complex<double>[N];
	std::complex<double> *evecs = new std::complex<double>[N * N];
	size_t nrot;
	size_t MaxIterations = (int)(2 + 2.8 * log((double)N) / log(2.0));
	if(MaxIterations < 8)
    {
        MaxIterations = 8;
    }
    Console::WriteLine(string("Eigensystem solve, sweeps: ") + string(MaxIterations));
	RNP::Eigensystem(N, A, N, evals, evecs, N, MaxIterations, &nrot);
	
	Eigenvalues.resize(N);
    Eigenvectors.resize(N, N);
    for(UINT Col = 0; Col < N; Col++)
	{
        Eigenvalues[Col] = evals[Col].real();
        for(UINT Row = 0; Row < N; Row++)
        {
            Eigenvectors.Cell(Row, Col) = evecs[Col * N + Row].real();
        }
    }

    delete [] A;
	delete [] evecs;
	delete [] evals;

    return true;
}

template<class T>
bool DenseMatrix<T>::EigenSystemTNT(vector<T> &Eigenvalues, DenseMatrix<T> &Eigenvectors) const
{
    PersistentAssert(Square(), "Eigensystem requires a square matrix");
    const UINT N = _rows;
    
    TNT::Array2D<T> TNTMatrix(N, N);
    for(UINT Row = 0; Row < N; Row++)
    {
        for(UINT Col = 0; Col < N; Col++)
        {
            TNTMatrix[Row][Col] = Cell(Row, Col);
        }
    }
    JAMA::Eigenvalue<T> eigensystem(TNTMatrix);

    eigensystem.getV(TNTMatrix);
    Eigenvectors.resize(N, N);
    for(UINT Row = 0; Row < N; Row++)
    {
        for(UINT Col = 0; Col < N; Col++)
        {
            Eigenvectors(Row, N - 1 - Col) = TNTMatrix[Row][Col];
        }
    }

    TNT::Array1D<T> TNTArray(N);
    eigensystem.getRealEigenvalues(TNTArray);
    
    Eigenvalues.resize(N);
    for(UINT eigenvalueIndex = 0; eigenvalueIndex < N; eigenvalueIndex++)
    {
        Eigenvalues[eigenvalueIndex] = TNTArray[N - 1 - eigenvalueIndex];
    }

    return true;
}

#define VTK_ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
        a[k][l]=h+s*(g-h*tau)

#define VTK_MAX_ROTATIONS 40

template<class T>
bool DenseMatrix<T>::EigenSystem(vector<T> &Eigenvalues, DenseMatrix<T> &Eigenvectors) const
{
    const UINT Dimension = _rows;
	Eigenvalues.resize(Dimension);
	Eigenvectors.resize(Dimension, Dimension);
	vector<T*> EigenvectorList(Dimension);
	for(UINT DimensionIndex = 0; DimensionIndex < Dimension; DimensionIndex++)
	{
		EigenvectorList[DimensionIndex] = Eigenvectors[DimensionIndex];
	}
    return EigenSystem(Eigenvalues.CArray(), EigenvectorList.CArray());
}

template<class T>
string DenseMatrix<T>::EigenTest(const vector<T> &Eigenvalues, const DenseMatrix<T> &Eigenvectors) const
{
    const UINT Dimension = _rows;
	vector<const T*> EigenvectorList(Dimension);
	for(UINT DimensionIndex = 0; DimensionIndex < Dimension; DimensionIndex++)
	{
		EigenvectorList[DimensionIndex] = Eigenvectors[DimensionIndex];
	}
    return EigenTest(Eigenvalues.CArray(), EigenvectorList.CArray());
}

//
// Jacobi iteration for the solution of eigenvectors/eigenvalues of a nxn
// real symmetric matrix. Square nxn matrix a; size of matrix in n;
// output eigenvalues in w; and output eigenvectors in Eigenvectors. Resulting
// eigenvalues/vectors are sorted in decreasing order; eigenvectors are
// normalized.
//
// Code modified from VTK vtkJacobiN function
//
template<class T>
bool DenseMatrix<T>::EigenSystem(T *Eigenvalues, T **Eigenvectors) const
{
    PersistentAssert(Square() && _rows >= 2, "Invalid matrix dimensions");
    int i, j, k, iq, ip, numPos, n = int(_rows);
    double tresh, theta, tau, t, sm, s, h, g, c, tmp;
    double bspace[4], zspace[4];
    double *b = bspace;
    double *z = zspace;

    //
    // Jacobi iteration destroys the matrix so create a temporary copy
    //
    DenseMatrix<double> a(_rows);
    for(UINT Row = 0; Row < _rows; Row++)
    {
        for(UINT Col = 0; Col < _cols; Col++)
        {
            a.Cell(Row, Col) = Cell(Row, Col);
        }
    }

    //
    // only allocate memory if the matrix is large
    //
    if (n > 4)
    {
        b = new double[n];
        z = new double[n]; 
    }

    //
    // initialize
    //
    for (ip=0; ip<n; ip++) 
    {
        for (iq=0; iq<n; iq++)
        {
            Eigenvectors[ip][iq] = 0.0;
        }
        Eigenvectors[ip][ip] = 1.0;
    }
    for (ip=0; ip<n; ip++) 
    {
        b[ip] = a[ip][ip];
        Eigenvalues[ip] = T(a[ip][ip]);
        z[ip] = 0.0;
    }

    // begin rotation sequence
    for (i=0; i<VTK_MAX_ROTATIONS; i++) 
    {
        sm = 0.0;
        for (ip=0; ip<n-1; ip++) 
        {
            for (iq=ip+1; iq<n; iq++)
            {
                sm += fabs(a[ip][iq]);
            }
        }
        if (sm == 0.0)
        {
            break;
        }

        if (i < 3)                                // first 3 sweeps
        {
            tresh = 0.2*sm/(n*n);
        }
        else
        {
            tresh = 0.0;
        }

        for (ip=0; ip<n-1; ip++) 
        {
            for (iq=ip+1; iq<n; iq++) 
            {
                g = T(100.0*fabs(a[ip][iq]));

                // after 4 sweeps
                if (i > 3 && (fabs(Eigenvalues[ip])+g) == fabs(Eigenvalues[ip])
                          && (fabs(Eigenvalues[iq])+g) == fabs(Eigenvalues[iq]))
                {
                    a[ip][iq] = 0.0;
                }
                else if (fabs(a[ip][iq]) > tresh) 
                {
                    h = Eigenvalues[iq] - Eigenvalues[ip];
                    if ( (fabs(h)+g) == fabs(h))
                    {
                        t = (a[ip][iq]) / h;
                    }
                    else 
                    {
                        theta = 0.5*h / (a[ip][iq]);
                        t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
                        if (theta < 0.0)
                        {
                            t = -t;
                        }
                    }
                    c = 1.0 / sqrt(1+t*t);
                    s = t*c;
                    tau = s/(1.0+c);
                    h = t*a[ip][iq];
                    z[ip] -= h;
                    z[iq] += h;
                    Eigenvalues[ip] -= T(h);
                    Eigenvalues[iq] += T(h);
                    a[ip][iq] = 0.0;

                    // ip already shifted left by 1 unit
                    for (j = 0;j <= ip-1;j++) 
                    {
                        VTK_ROTATE(a,j,ip,j,iq);
                    }
                    // ip and iq already shifted left by 1 unit
                    for (j = ip+1;j <= iq-1;j++) 
                    {
                        VTK_ROTATE(a,ip,j,j,iq);
                    }
                    // iq already shifted left by 1 unit
                    for (j=iq+1; j<n; j++) 
                    {
                        VTK_ROTATE(a,ip,j,iq,j);
                    }
                    for (j=0; j<n; j++) 
                    {
#pragma warning ( disable : 4244 )
                        VTK_ROTATE(Eigenvectors,j,ip,j,iq);
#pragma warning ( default : 4244 )
                    }
                }
            }
        }

        for (ip=0; ip<n; ip++) 
        {
            b[ip] += z[ip];
            Eigenvalues[ip] = T(b[ip]);
            z[ip] = 0.0;
        }
    }

    if ( i >= VTK_MAX_ROTATIONS )
    {
        Utility::MessageBox("VTK_MAX_ROTATIONS exceeded");
        //SignalError("VTK_MAX_ROTATIONS exceeded");
        //return false;
    }

    // sort eigenfunctions                 these changes do not affect accuracy 
    for (j=0; j<n-1; j++)                  // boundary incorrect
    {
        k = j;
        tmp = Eigenvalues[k];
        for (i=j+1; i<n; i++)                // boundary incorrect, shifted already
        {
            if (Eigenvalues[i] >= tmp)                   // why exchage if same?
            {
                k = i;
                tmp = Eigenvalues[k];
            }
        }
        if (k != j) 
        {
            Eigenvalues[k] = Eigenvalues[j];
            Eigenvalues[j] = T(tmp);
            for (i=0; i<n; i++) 
            {
                tmp = Eigenvectors[i][j];
                Eigenvectors[i][j] = Eigenvectors[i][k];
                Eigenvectors[i][k] = T(tmp);
            }
        }
    }

    //
    // insure eigenvector consistency (i.e., Jacobi can compute vectors that
    // are negative of one another (.707,.707,0) and (-.707,-.707,0). This can
    // reek havoc in hyperstreamline/other stuff. We will select the most
    // positive eigenvector.
    //
    int ceil_half_n = (n >> 1) + (n & 1);
    for (j=0; j<n; j++)
    {
        for (numPos=0, i=0; i<n; i++)
        {
            if ( Eigenvectors[i][j] >= 0.0 )
            {
                numPos++;
            }
        }
        //    if ( numPos < ceil(double(n)/double(2.0)) )
        if ( numPos < ceil_half_n)
        {
            for(i=0; i<n; i++)
            {
                Eigenvectors[i][j] *= -1.0;
            }
        }
    }

    if (n > 4)
    {
        delete [] b;
        delete [] z;
    }
    return true;
}

template<class T>
string DenseMatrix<T>::EigenTest(const T *Eigenvalues, const T **Eigenvectors) const
{
    const bool Verbose = false;
    string Description;
    PersistentAssert(Square() && _rows >= 2, "Invalid matrix dimensions");
    vector<T> Eigenvector(_rows), result;
    double MaxError = 0.0;
    for(UINT EigenIndex = 0; EigenIndex < _rows; EigenIndex++)
    {
        for(UINT ElementIndex = 0; ElementIndex < _rows; ElementIndex++)
        {
            Eigenvector[ElementIndex] = Eigenvectors[ElementIndex][EigenIndex];
        }
        DenseMatrix<T>::Multiply(result, *this, Eigenvector);
        
        double Error = 0.0;
        T CurEigenvalue = Eigenvalues[EigenIndex];
        for(UINT ElementIndex = 0; ElementIndex < _rows; ElementIndex++)
        {
            Error += fabs(Eigenvector[ElementIndex] * CurEigenvalue - result[ElementIndex]);
        }
        if(Verbose)
        {
            Description += string("Eigenvector ") + string(EigenIndex) + string(" absolute error: ") + string(Error) + string("\n");
        }
        MaxError = Math::Max(Error, MaxError);
    }
    Description += string("Max eigenvector error: ") + string(MaxError) + string("\n");
    return Description;
}

template<class T>
T DenseMatrix<T>::Determinant() const
{
    Assert(Square(), "Determinant called on non-square matrix");
    if(_rows == 1)
    {
        return Cell(0, 0);
    }
    else if(_rows == 2)
    {
        return Cell(0, 0) * Cell(1, 1) - Cell(1, 0) * Cell(0, 1);
    }
    else if(_rows == 3)
    {
        return (Cell(0, 0) * Cell(1, 1) * Cell(2, 2) + Cell(0, 1) * Cell(1, 2) * Cell(2, 0) + Cell(0, 2) * Cell(1, 0) * Cell(2, 1)) -
               (Cell(2, 0) * Cell(1, 1) * Cell(0, 2) + Cell(2, 1) * Cell(1, 2) * Cell(0, 0) + Cell(2, 2) * Cell(1, 0) * Cell(0, 1));
    }
    else
    {
		return DeterminantLaplaceExpansion();
    }
}

template<class T>
T DenseMatrix<T>::DeterminantLaplaceExpansion() const
{
	// Recursive computation via Laplace Expansion along the first row
	T det = 0.0;
	UINT i = 0;
	DenseMatrix<T> minor;
	for (UINT j = 0; j < _cols; j++)
	{
		T b = Cell(i, j);
		if (b == (T)0.0) continue;
		int coeff = ((i+j) % 2 == 0 ? 1 : -1);
		MinorMatrix(i, j, minor);
		det += b * coeff * minor.Determinant();
	}
	return det;
}

template<class T>
void DenseMatrix<T>::MinorMatrix(UINT Row, UINT Col, DenseMatrix<T>& result) const
{
	result.resize(_rows - 1, _cols - 1);
	UINT i = 0;
	for (UINT ii = 0; ii < _rows; ii++)
	{
		if (ii == Row) continue;
		UINT j = 0;
		for (UINT jj = 0; jj < _cols; jj++)
		{
			if (jj == Col) continue;
			result(i,j) = Cell(ii,jj);
			j++;
		}
		i++;
	}
}

template<class T>
DenseMatrix<T>DenseMatrix<T>::Inverse() const
{
    DenseMatrix<T>result = *this;
    result.InvertInPlace();
    return result;
}

template<class T>
void DenseMatrix<T>::InvertInPlace()
{
    Assert(Square(), "DenseMatrix<T>::Invert called on non-square matrix");
    for (UINT i = 1; i < _rows; i++)
    {
        Cell(0, i) /= Cell(0, 0);
    }

    for (UINT i = 1; i < _rows; i++)
    {
        //
        // do a column of L
        //
        for (UINT j = i; j < _rows; j++)
        {
            T Sum = 0.0f;
            for (UINT k = 0; k < i; k++)  
            {
                Sum += Cell(j, k) * Cell(k, i);
            }
            Cell(j, i) -= Sum;
        }
        if (i == _rows - 1)
        {
            continue;
        }

        //
        // do a row of U
        //
        for (UINT j = i + 1; j < _rows; j++)
        {
            T Sum = 0.0f;
            for (UINT k = 0; k < i; k++)
                Sum += Cell(i, k) * Cell(k, j);
            Cell(i, j) = (Cell(i, j) - Sum) / Cell(i, i);
        }
    }

    //
    // invert L
    //
    for (UINT i = 0; i < _rows; i++)
        for (UINT j = i; j < _rows; j++)
        {
            T Sum = 1.0f;
            if ( i != j )
            {
                Sum = 0.0f;
                for (UINT k = i; k < j; k++ ) 
                {
                    Sum -= Cell(j, k) * Cell(k, i);
                }
            }
            Cell(j, i) = Sum / Cell(j, j);
        }

    //
    // invert U
    //
    for (UINT i = 0; i < _rows; i++)
        for (UINT j = i; j < _rows; j++)
        {
            if ( i == j )
            {
                continue;
            }
            T Sum = 0.0f;
            for (UINT k = i; k < j; k++)
            {
                T Val = 1.0f;
                if(i != k)
                {
                    Val = Cell(i, k);
                }
                Sum += Cell(k, j) * Val;
            }
            Cell(i, j) = -Sum;
        }
    
    //
    // final inversion
    //
    for (UINT i = 0; i < _rows; i++)
    {
        for (UINT j = 0; j < _rows; j++)
        {
            T Sum = 0.0f;
            UINT Larger = j;
            if(i > j)
            {
                Larger = i;
            }
            for (UINT k = Larger; k < _rows; k++)
            {
                T Val = 1.0f;
                if(j != k)
                {
                    Val = Cell(j, k);
                }
                Sum += Val * Cell(k, i);
            }
            Cell(j, i) = Sum;
        }
    }
    //Assert(ElementsValid(), "Degenerate Matrix inversion.");
}

template<class T>
void DenseMatrix<T>::Identity()
{
    Assert(Square(), "DenseMatrix<T>::Identity called on non-square Matrix4.");
    for(UINT i = 0; i < _rows; i++)
    {
        for(UINT i2 = 0; i2 < _rows; i2++)
        {
            if(i == i2)
            {
                Cell(i, i2) = 1.0f;
            }
            else
            {
                Cell(i, i2) = 0.0f;
            }
        }
    }
}

template<class T>
void DenseMatrix<T>::Identity(UINT Size)
{
    Allocate(Size, Size);
    Identity();
}

template<class T>
bool DenseMatrix<T>::Square() const
{
    return (_rows == _cols);
}

template<class T>
bool DenseMatrix<T>::ElementsValid()
{
    for(UINT Row = 0; Row < _rows; Row++)
    {
        for(UINT Col = 0; Col < _cols; Col++)
        {
            if(Cell(Row, Col) != Cell(Row, Col))
            {
                return false;
            }
        }
    }
    return true;
}

template<class T>
DenseMatrix<T> DenseMatrix<T>::Transpose() const
{
    DenseMatrix<T> result;
    result.Allocate(_cols, _rows);
    for(UINT i = 0; i < _rows; i++)
    {
        for(UINT i2 = 0; i2 < _cols; i2++)
        {
            result.Cell(i2, i) = Cell(i, i2);
        }
    }
    return result;
}

/*ostream& operator << (ostream &os, const DenseMatrix<T>&m)
{
    UINT n = m.RowCount();
    //os << setprecision(8) << setiosflags(ios::right | ios::showpoint);
    os << "{";
    for(UINT i=0;i<n;i++)
    {
        os << "{";
        for(UINT i2=0;i2<n;i2++)
        {
            os << m[i][i2];
            if(i2 != n - 1) os << ", ";
        }
        
        os << "}";
        if(i != n - 1) os << ", ";
        os << endl;
    }
    os << "}";
    return os;
}*/

template<class T>
ostream& operator << (ostream &os, const DenseMatrix<T>&m)
{
    Assert(m.Square(), "Output not correct for non-square matrices.");
    UINT n = m.RowCount();
    os << setprecision(8) << setiosflags(ios::right | ios::showpoint);
    for(UINT i = 0; i < n; i++)
    {
        for(UINT i2 = 0; i2 < n; i2++)
        {
            os << setw(12) << m[i][i2];
        }
        os << endl;
    }
    return os;
}

template<class T>
void DenseMatrix<T>::Multiply(vector<T> &result, const DenseMatrix<T>&left, const vector<T> &right)
{
    Assert(left.ColCount() == right.size(), "Invalid dimensions in DenseMatrix<T>::Multiply");
    result.resize(left.RowCount());
    
    for(UINT Row = 0; Row < result.size(); Row++)
    {
        T Total = 0.0;
        for(UINT Index = 0; Index < left.ColCount(); Index++)
        {
            Total += left.Cell(Row, Index) * right[Index];
        }
        result[Row] = Total;
    }
}

template<class T>
void DenseMatrix<T>::Multiply(T *result, const DenseMatrix<T>&left, const T *right)
{
    const UINT colCount = left.ColCount();
    const UINT rowCount = left.RowCount();
    for(UINT row = 0; row < rowCount; row++)
    {
        T total = 0.0;
        for(UINT col = 0; col < colCount; col++)
        {
            total += left(row, col) * right[col];
        }
        result[row] = total;
    }
}

template<class T>
void DenseMatrix<T>::Multiply(DenseMatrix<T>&result, const DenseMatrix<T>&left, const DenseMatrix<T>&right)
{
    Assert(left.ColCount() == right.RowCount(), "Invalid matrix dimensions on SetProduct");
    const UINT RowCount = left.RowCount();
    const UINT ColCount = right.ColCount();
    result.Allocate(RowCount, ColCount);
    for(UINT RowIndex = 0; RowIndex < RowCount; RowIndex++)
    {
        for(UINT ColIndex = 0; ColIndex < ColCount; ColIndex++)
        {
            T Total = 0.0;
            for(UINT InnerIndex = 0; InnerIndex < left.ColCount(); InnerIndex++)
            {
                Total += left.Cell(RowIndex, InnerIndex) * right.Cell(InnerIndex, ColIndex);
            }
            result[RowIndex][ColIndex] = Total;
        }
    }
}

template<class T>
void DenseMatrix<T>::MultiplyMMTranspose(DenseMatrix<T>&result, const DenseMatrix<T>&M)
{
    const UINT RowCount = M.RowCount();
    const UINT ColCount = M.ColCount();
    result.Allocate(RowCount, RowCount);
    for(UINT i = 0; i < RowCount; i++)
    {
        for(UINT i2 = 0; i2 < RowCount; i2++)
        {
            T Total = 0.0;
            for(UINT i3 = 0; i3 < ColCount; i3++)
            {
                Total += M.Cell(i, i3) * M.Cell(i2, i3);
            }
            result.Cell(i, i2) = Total;
        }
    }
}

template<class T>
void DenseMatrix<T>::MultiplyInPlace(DenseMatrix<T>&result, T right)
{
    for(UINT Row = 0; Row < result.RowCount(); Row++)
    {
        for(UINT Col = 0; Col < result.ColCount(); Col++)
        {
            result[Row][Col] *= right;
        }
    }
}

template<class T>
vector<T> DenseMatrix<T>::LinearRegression(const DenseMatrix<T> &X, const vector<T> &Y)
{    
    DenseMatrix<T> XTranspose = X.Transpose();
    DenseMatrix<T> XTX = XTranspose * X;
    DenseMatrix<T> Inverse = XTX.Inverse();
    return Inverse * (XTranspose * Y);
    //return (XTranspose * X).Inverse() * (XTranspose * Y);
}

template<class T>
vector<T> operator * (const DenseMatrix<T> &left, const vector<T> &right)
{
    vector<T> result;
    DenseMatrix<T>::Multiply(result, left, right);
    return result;
}

template<class T>
DenseMatrix<T> operator * (const DenseMatrix<T>&left, const DenseMatrix<T>&right)
{
    DenseMatrix<T>result;
    DenseMatrix<T>::Multiply(result, left, right);
    return result;
}

template<class T>
DenseMatrix<T>operator * (const DenseMatrix<T>&left, T right)
{
    DenseMatrix<T>Return(left.RowCount(), left.ColCount());
    for(UINT Row = 0; Row < left.RowCount(); Row++)
    {
        for(UINT Col = 0; Col < left.ColCount(); Col++)
        {
            Return[Row][Col] = left[Row][Col] * right;
        }
    }
    return Return;
}

template<class T>
DenseMatrix<T>operator * (T right, const DenseMatrix<T>&left)
{
    DenseMatrix<T>Return(left.RowCount(), left.ColCount());
    for(UINT Row = 0; Row < left.RowCount(); Row++)
    {
        for(UINT Col = 0; Col < left.ColCount(); Col++)
        {
            Return[Row][Col] = left[Row][Col] * right;
        }
    }
    return Return;
}

template<class T>
DenseMatrix<T>operator + (const DenseMatrix<T>&left, const DenseMatrix<T>&right)
{
    Assert(left.RowCount() == right.RowCount() && left.ColCount() == right.ColCount(), "Invalid dimensions on Matrix4 addition.");
    DenseMatrix<T>Return(left.RowCount(), left.ColCount());
    for(UINT Row = 0; Row < left.RowCount(); Row++)
    {
        for(UINT Col = 0; Col < left.ColCount(); Col++)
        {
            Return[Row][Col] = left[Row][Col] + right[Row][Col];
        }
    }
    return Return;
}

template<class T>
DenseMatrix<T>operator - (const DenseMatrix<T>&left, const DenseMatrix<T>&right)
{
    Assert(left.RowCount() == right.RowCount() && left.ColCount() == right.ColCount(), "Invalid dimensions on Matrix4 addition.");
    DenseMatrix<T>Return(left.RowCount(), left.ColCount());
    for(UINT Row = 0; Row < left.RowCount(); Row++)
    {
        for(UINT Col = 0; Col < left.ColCount(); Col++)
        {
            Return[Row][Col] = left[Row][Col] - right[Row][Col];
        }
    }
    return Return;
}
